/*============================================================================*/
/*                        Tortoise Team			                              */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* C Source:         %APP_Linear_Camera.c%
* Instance:         RPL_1
* %version:         1.2 %
* %created_by:      Andres Torres Garcia %
* %date_created:    Sunday Sep  30 14:38:03 2012 %
*=============================================================================*/
/* DESCRIPTION : C source file for the application layer of the Linear Camera */
/*============================================================================*/
/* FUNCTION COMMENT : This file contains the declarations and definitions of  */
/*					  the variables, and functions of the application layer of*/													  
/*					  the camera.											  */ 
/*					  	  											  		  */
/* 					                      									  */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 30/09/2012  |                               | Andres Torres    */
/*  1.1      | 04/10/2012  |                               | Andres Torres    */
/*  1.2      | 07/10/2012  |                               | Andres Torres    */
/*  1.3      | 13/11/2012  |                               | Andres Torres    */
/* Integration under Continuus CM                                             */
/*============================================================================*/

/* Includes */
/* -------- */
#include "APP_Linear_Camera.h"
#include "HAL_Linear_Camera.h"
#include "HAL_Btn.h"

/* Functions macros, constants, types and datas         */
/* ---------------------------------------------------- */
/* Functions macros */

/*!
  \def GOAL
  Pattern for a goal.
*/
#define GOAL				42		//0010-1010 - 7 shifts
/*!
  \def GOAL_2
  Pattern for a goal.
*/
#define GOAL_2				0x15	//0001-0101 - 5 shifts
/*!
  \def BLACK_LINE
  Pattern for a black line.
*/
#define BLACK_LINE	 		2		//0000-0010 - 3 shifts
/*!
  \def PIN_DEBUG
  PIN for debug.
*/
#define PIN_DEBUG			1
/*!
  \def CHAR_BIT
  Useful for the function getAbs
*/
#define CHAR_BIT 			8

/* LONG and STRUCTURE constants */

/**
* This structure contains useful information for the pixel patterns.
*/
typedef struct Pattern_info {
	
	/*@{*/
	T_UBYTE width[20]; /**< Contains the width of each pattern. */
	T_UBYTE value; /**< Contains the pattern in binary. */
	
	T_UBYTE valid; /**< Contains number of valid values. */
	/*@}*/
	
} Pattern_info;

/*==================================================*/ 
/* Definition of constants                          */
/*==================================================*/ 

/** \brief Tolerance range for the width of the line */
const T_SBYTE sb_RANGE = 6;

/*======================================================*/ 
/* Definition of RAM variables                          */
/*======================================================*/

/** \brief Left corner of the black line */
T_UBYTE ub_Left_Corner = 0;

/** \brief Right corner of the black line */
T_UBYTE ub_Right_Corner = 0;

/** \brief Width of the black line */
T_UBYTE ub_Black_Strip_Width = 0;
/** \brief The center of the black line */
T_UBYTE ub_Black_Strip_Center = 0;

/** \brief Flat that determines if we found a goal. */
T_UBYTE ub_IsGoal = 0;

/** \brief Flag that determines the time to proccess the pixels of the linear camera. */
T_UBYTE ub_Can_Process_Pixels = 0;

/** \brief Pixels of the camera binarize */
T_UBYTE ub_Bin_Pixels[NUM_PIXELS];

/** \brief Array of line centers. */
T_UBYTE ub_Line_Average[NUM_PIXELS];

extern T_SWORD sw_Pixels[NUM_PIXELS];

/** \brief The threshold that helps us to binarize the pixels. */
T_SWORD sw_Threshold = 0;

/** \brief Holds important information about the pattern. */
Pattern_info pat_info;

/** \brief Calibrate value of the width of the line. */
static T_SBYTE sb_BLACK_STRIP_WIDTH_MEASU = 20;

/** \brief Max pixel value */
static T_SWORD sw_max = 0;

/** \brief Min pix value */
static T_SWORD sw_min = 1023;

/*======================================================*/ 
/* close variable declaration sections                  */
/*======================================================*/ 

/* Private defines */

/* Private functions prototypes */
/* ---------------------------- */
void vfn_saveLineCenterPosition(void);
void vfn_binarize(void);
void vfn_detectLine(void);
void vfn_processPixels(void);
void vfn_calibrateThreshold(void);
void vfn_calibrateLine(void);
void vfn_isValidLine(T_UBYTE width, T_UBYTE center, T_UBYTE left, T_UBYTE right);

/* Exported functions prototypes */
/* ----------------------------- */
void vfnInterrupToProc_And_CaptPix(void);
void vfnCalibrateLinearCamera(void);
T_UBYTE ub_PowerOfTwo( T_UBYTE ub_x );
T_UWORD getAbs(T_SWORD num);

/* Inline functions */
/* ---------------- */

/* Private functions */
/* ----------------- */

/**************************************************************
 *  Name                 : 	vfn_saveLineCenterPosition
 *  Description          :	Store different reads of the line center in an array.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Store different reads of the line center in an array.
/////////////////////////////////////////////////////////////////////////
 void vfn_saveLineCenterPosition(void)
 {
 	T_UBYTE i;

	for(i = CAMERA_AVG; i > 0; i--)
	{
		ub_Line_Average[i] = ub_Line_Average[i-1];
	}	
 }
 
 /**************************************************************
 *  Name                 : 	vfn_detectLine
 *  Description          :	Try to detect the line or the goal.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Try to detect the line or the goal.
/////////////////////////////////////////////////////////////////////////
void vfn_detectLine(void)
{
	T_UBYTE width = 0;
	T_UBYTE center = 0;

	//Shifting to the right the array of line centers.
	vfn_saveLineCenterPosition();
	
	ub_IsGoal = 0;
	
	//Is a goal?
	if( pat_info.value ==  GOAL && pat_info.valid == 7)
	{
		width = pat_info.width[3] - pat_info.width[2];
		center = width / 2 + pat_info.width[2];
		ub_IsGoal = 1;
		
		vfn_isValidLine( width, center, pat_info.width[2], pat_info.width[3] - 1);
	}//Is a line?
	else if( pat_info.value ==  BLACK_LINE && pat_info.valid == 3 )
	{
		width = pat_info.width[1] - pat_info.width[0];
		center = width / 2 + pat_info.width[0];
		
		vfn_isValidLine( width, center, pat_info.width[0], pat_info.width[1] - 1);
	}//Is a goal?
	else if( pat_info.value ==  GOAL_2 && pat_info.valid == 5 )
	{
		width = pat_info.width[2] - pat_info.width[1];
		center = width / 2 + pat_info.width[1];
		ub_IsGoal = 1;
		
		vfn_isValidLine( width, center, pat_info.width[1], pat_info.width[2] - 1);
	}
}

 /**************************************************************
 *  Name                 : 	vfn_isValidLine
 *  Description          :	Check if the line is a valid one and if it is, assign values.
 *  Parameters           :  [Input: ub_width, ub_center, ub_left ,ub_right, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Check if the line is a valid one and if it is, assign values.
/// \param ub_width width of the line.
/// \param ub_center center of the line.
/// \param ub_left left corner of the line.
/// \param ub_right right corner of the line.
/////////////////////////////////////////////////////////////////////////
void vfn_isValidLine(T_UBYTE ub_width, T_UBYTE ub_center, T_UBYTE ub_left, T_UBYTE ub_right)
{
	if( ( ( sb_BLACK_STRIP_WIDTH_MEASU - sb_RANGE ) <= ub_width ) && ( ( sb_BLACK_STRIP_WIDTH_MEASU + sb_RANGE ) >= ub_width ) )
	{
		ub_Black_Strip_Width = ub_width;
		
		ub_Line_Average[0] = ub_Black_Strip_Center = ub_center;
		
		ub_Left_Corner = ub_left;
		ub_Right_Corner = ub_right;
	}
	else
	{
		ub_IsGoal = 0;
	}
}

/**************************************************************
 *  Name                 : 	vfnProcess_Pixels
 *  Description          :	Process the pixels of the linear camera and detect the line
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Process the pixels of the linear camera and detect the line
/////////////////////////////////////////////////////////////////////////
void vfn_processPixels(void)
{	
	vfn_binarize();
	vfn_detectLine();
}

/**************************************************************
 *  Name                 : 	vfn_binarize
 *  Description          :	Create a pattern from the pixels.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Create a pattern from the pixels.
/////////////////////////////////////////////////////////////////////////
void vfn_binarize(void)
{
	T_UBYTE i = 0;
	T_SWORD sw_numComp = sw_Pixels[0];
	T_UBYTE j = 0;
	T_UBYTE width = 0;
	
	T_SWORD diff = 0;
	T_SWORD last_diff = 0;
	
	pat_info.value = 0;
	pat_info.valid = 0;
	pat_info.width[j] = 0;
	
	for( i = 0; i < NUM_PIXELS; i++)
	{	
		width++;
		
		diff = sw_numComp - sw_Pixels[i];
		
		//Is the difference is enough?
		if( getAbs( diff ) >= sw_Threshold )
		{
			sw_numComp = sw_Pixels[i];
			last_diff = diff;
			pat_info.value = pat_info.value << 1;
			pat_info.valid++;
			pat_info.width[j] = width;
			
			j++;
			
			if( diff < 0 )
			{		
				//Adding 1
				pat_info.value |= 1;
			}
		}
	}
	
	pat_info.valid++;
	pat_info.value = pat_info.value << 1;
	
	if(last_diff > 0)
	{
		//Adding 1
		pat_info.value |= 1;
	}
	
	pat_info.width[j] = width;
}

/**************************************************************
 *  Name                 : 	vfn_calibrateLine
 *  Description          :	Measure the line in pixels
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Measure the line in pixels
/////////////////////////////////////////////////////////////////////////
void vfn_calibrateLine(void)
{
	//Create the pattern.
	vfn_binarize();
	
	//Is a black line?
	if( pat_info.value ==  BLACK_LINE && pat_info.valid == 3 )
	{
		ub_Left_Corner = pat_info.width[0];
		ub_Right_Corner = pat_info.width[1] - 1;
	
		sb_BLACK_STRIP_WIDTH_MEASU = ub_Black_Strip_Width = pat_info.width[1] - pat_info.width[0];
		ub_Black_Strip_Center = ub_Black_Strip_Width / 2 + pat_info.width[0];
	}
}

/**************************************************************
 *  Name                 : 	vfn_calibrateThreshold
 *  Description          :	Calculate the threshold automatically
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Calculate the threshold automatically
/////////////////////////////////////////////////////////////////////////
void vfn_calibrateThreshold(void)
{
	T_SWORD sw_tmp = 0;
	
	T_UBYTE i;
	
	sw_max = 0;
	sw_min = 1023;
	
	for( i = 0; i < NUM_PIXELS; i++)
	{
		if( sw_Pixels[i]  > sw_max )
		{
			sw_max = sw_Pixels[i];
		}
		
		if( sw_Pixels[i]  < sw_min )
		{
			sw_min = sw_Pixels[i];
		}
	}
	
	sw_tmp = sw_max - sw_min;
	
	sw_tmp = sw_tmp / 2;
	
	sw_Threshold = sw_tmp + sw_tmp / 2;
}

/* Exported functions */
/* ------------------ */


/**************************************************************
 *  Name                 :	ub_PowerOfTwo
 *  Description          :	Elevates 2 to the power of x
 *  Parameters           :  [Input: ub_x, Output, Input / output]
 *  Return               :	T_UBYTE
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Elevates 2 to the power of x
/// \param ub_x power of x
/// \return result
/////////////////////////////////////////////////////////////////////////
T_UBYTE ub_PowerOfTwo( T_UBYTE ub_x )
{
	T_UBYTE ub_y = 1;
	
	ub_x--;

	while( ub_x )
	{
		ub_y = ub_y << 1;
		ub_x--;
	}
}


/**************************************************************
 *  Name                 :	getAbs
 *  Description          :	Gets the absolute value of num.
 *  Parameters           :  [Input: sw_num, Output, Input / output]
 *  Return               :	T_UWORD
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Gets the absolute value of num.
/// \param sw_num the number that you want to get its absolut.
/// \return the absolute number of sw_num
/////////////////////////////////////////////////////////////////////////
T_UWORD getAbs(T_SWORD sw_num)
{
	T_SWORD const sw_mask = sw_num >> (sizeof(T_SWORD) * CHAR_BIT - 1);
  	return ((sw_num ^ sw_mask) - sw_mask);
}

/**************************************************************
 *  Name                 :	vfnCalibrateLinearCamera
 *  Description          :	Calibrate the Linear Camera.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Calibrate the Linear Camera.
/////////////////////////////////////////////////////////////////////////
void vfnCalibrateLinearCamera(void)
{
	static T_UBYTE ub_calibrationState = 0;
	
	switch(ub_calibrationState)
	{
		case 0:					
				if(Check_Btn_Press(BTN_3))
				{
					ub_calibrationState = 1;
					
					APPLeds_Led4_app();
				}
				else
				{
					vfn_calibrateThreshold();
				}
				
		break;
		
		case 1:
				if(Check_Btn_Press(BTN_3))
				{
					ub_calibrationState = 2;
					
					APPLeds_Led4_app();
				}
				else
				{
					vfn_calibrateLine();
				}
				
		break;
		
		case 2:
		
				//Do nothing
		
		break;
		
	}
}

/**************************************************************
 *  Name                 : 	vfnInterrupToProcPix
 *  Description          :	Interruption to proccess the pixels of the linear camera.
 *  Parameters           :  [Input, Output, Input / output]
 *  Return               :	void
 *  Critical/explanation :    No
 **************************************************************/
///////////////////////////////////////////////////////////////////////
/// \brief Interruption to proccess the pixels of the linear camera.
/////////////////////////////////////////////////////////////////////////
void vfnInterrupToProc_And_CaptPix(void)
{
	vfnInterrupToCapturePix();
	
	if(ub_Can_Process_Pixels)
	{
		//IO_ToggleOutput(PIN_DEBUG);
		vfn_processPixels();
		//IO_ToggleOutput(PIN_DEBUG);
	}
}